Model-based Vehicle State Estimation Using Previewed Road Geometry and Noisy Sensors

This paper proposes a method for using previewed road geometry from a high-fidelity map to improve estimates of planar vehicle states in the presence of unmodeled sensor bias errors. Using well-established, linear models for representing human driver behavior and for planar vehicle states, a causal link between previewed road geometry and vehicle states can be derived. Cast as an augmented, closed-loop linear system, the total driver-vehicle-road system’s states are estimated using a Kalman filter. Estimation results from this filter using simulated noisy measurements of vehicle states and map-based measurements of previewed road geometry are compared to standard Kalman filters with identical measurements of vehicle states alone. The effects of errors in driver modeling, vehicle nonlinearity, and measurement disturbances on the estimator’s fidelity are also examined and discussed. INTRODUCTION As computing power has become increasingly affordable in smaller and smaller packages, so have the inertial and position sensors common to the automotive world. Unfortunately for the designers of vehicle driver assist systems, however, most lowcost sensors still suffer from debilitating noise characteristics that make their use for vehicle tracking difficult. The outlook is less dire when absolute vehicle position is not required. For instance, commercial vehicle stability control algorithms have long relied on model-based estimation to make the most of available inertial sensing technology [1]. Model-based estimation with noisy sensors has essentially enabled the production and deployment of stability-control-equipped vehicles, but Address all correspondence to this author. with growing interest in vehicle autonomy and driver assist technologies like high-speed collision avoidance, slip detection and control, great interest in gaining sufficient knowledge of vehicle states from low-cost sensors remains [2]. In the case of autonomous vehicle guidance or in modeling of driver response, it is generally assumed that maps are available of the road geometry. Map information has already shown to be a useful tool in improving vehicle localization accuracy [3]. The key insight of this paper is that the steering input, calculated from a previewed road geometry, can be considered yet another sensor input to estimate vehicle state. This is a particularly low-cost data source, especially in contrast to the costs associated with high-quality sensing equipment that makes direct measurements of vehicle states like sideslip, lateral position within a lane, and true vehicle yaw angle possible. The present study augments the typical Kalman filter for vehicle state estimation by using multiple map measurements per time step to aid in reliable, drift-free state estimation. This is achieved by coupling road geometry to vehicle dynamics through a representative driver model. Results are encouraging, even though the models used are linear, subject to error, and the actual inertial sensors used on the simulated vehicle are subject to large amounts of error. The use of road preview in the state estimation problem offers marked improvement over the use of inertial sensors and GPS alone. The following pages outline an estimation paradigm that enables the use of high-fidelity geometric maps of roads as measurements in a model-based Kalman filter framework. The remainder of the paper is organized as follows: The following section gives a brief outline of the history and state of the art in driver modeling using linear models with preview and set the precedent for increased use of map information in vehicle state estimators. Then, a brief discussion outlines the driver-vehicle model used in the development of two model-based Kalman filters designed to estimate vehicle states with and without road preview information. The results of using these two types of estimators, both with a perfect vehicle model and in the presence of modeling error are discussed, and results from simulations of a vehicle traversing a 80kph (50 mph) double lane change maneuver follow. Linear Models of Automobile Drivers using Preview Vehicle driver modeling has been an important field of study for over 20 years. In fact, some modern, high-fidelity vehicle simulation software packages still make use of driver models that are over 30 years old [4]. In 1980, MacAdam applied an optimal fixed-point preview controller to vehicle lateral guidance in [5] and showed that the model agreed well with actual human driver behavior. A decade later, as a result of the PATH program at the beginning of the 1990s, researchers at U.C. Berkeley [6, 7] developed guidance laws for autonomous vehicle control. These control strategies also used feedforward control acting on previewed road curvature along with feedback to achieve vehicle path tracking. But instead of focusing on matching human driver behavior, the aim was to engineer solutions for autonomous vehicles that could be implemented on public highways. Researchers involved in this program, along with others, continued this vein of research through the 1990s [8–10]. This paper is not intended to be a comprehensive review of lateral vehicle control; the authors would like to refer readers to more comprehensive reviews on this topic in [11, 12]. Whether for driver modeling or for vehicle autonomy, nearly all of such research makes use of previewed information in one form or another. In other words, autonomous driving and driver models assume knowledge of what lies ahead of the driver in the vehicle steering task. Amongst the more or less successful linear driver models in the literature, at least two distinct schools of thought emerge. The first, consistent with [5], relies on a projection of the system states into the future. In a sense, even the applications of model-predictive control [13,14] follow this thread. The other seems to have grown out of an interest in applying methods from optimal preview and LQR suspension control [15]. Sharp and Prokop used previewed road geometry to drive the actions of their optimal preview steering controller in [16], and the authors’ work along these lines continued through the following decade in [17, 18]. While the controller proposed in [16] was probably not devised to model human behavior exactly, Pick and Cole were able to show that this type of controller approximates human behavior quite well [19], especially when neuromuscular dynamics are included. Pick and Cole also examined the mathematical relationship between predictive control theory and Linear Quadratic preview control theory in [12]. This is an enlightening read, and clearly shows how, under many circumstances, the two approaches can yield identical controllers. The authors also found that there are some instances where this is not possible, and the approaches give divergent results. For the present study, Sharp’s Optimal LQ steering controller will be used as the control model for the closed-loop driver-vehicle-road system. This structure is ideally suited to the current application, which seeks to utilize the control effort associated with the previewed map to better estimate current planar vehicle position, yaw rate, angular rate and lateral velocity. Vehicle state estimation with and without map information Estimating vehicle states using low-cost sensing equipment is hard, and forces many production driver assist systems to be quite conservative in anticipation of sensor error [2]. The relatively low signal-to-noise ratio of production sensors makes it challenging to measure vehicle states like sideslip, the angle between the the vehicle orientation and the vehicle’s total velocity vector, because sideslip has extremely small magnitudes under normal driving conditions. Many low-cost sensors suffer from severe bias instability, quantization effects, poor resistance to temperature and other environmental variability. As a result, the use of common low-cost inertial sensors in traditional Kinematic Kalman Filters (KKFs) is often out of the question, although success with vehicle sideslip estimation without a model using GPS and yaw gyro measurements was shown in [20]. Some researchers in the vehicle dynamics community have turned towards model-based estimators that make use of known vehicle dynamics to improve estimator accuracy [21, 22]. Some have even found success using model-based estimation techniques to estimate vehicle parameters and/or tire-pavement friction in real time [2, 23, 24]. In the application most similar to the current study, Mudaliar used a model-based Kalman filter in the design of a lane departure warning system [4], and the match between the filter and the simulated CarSim vehicle was exceptional. While model-based estimation can indeed improve state estimates using otherwise inferior sensors, relying on the model structure itself is a double-edged sword: the benefits are that the model dynamics constrain the estimator error to be consistent with expected behavior. The consequences are that modeling error, when left unchecked, can introduce artificially amplified errors in estimated states. One of the goals of the present study is to examine whether the detrimental effects of modeling error can be somewhat mitigated through the use of a map, which has the potential to offer nearly limitless measurements at any given time step, and with an extremely high degree of accuracy. Using maps for vehicle localization and state estimation is not a new idea. Recent work by the authors [3, 25–27] makes use of extremely compact maps of roads to localize a vehicle by using a measurement of its pitch angle alone. Alas, most of these studies tend to bring map information into a filter once every time step. If multiple measurements are available from a map at each time step, each coupled to the model slated for state estimation, accuracy is likely to improve. The above discussion on preview control suggests benefits for including multiple map measurements at a given time step in an estimation algorithm; because preview control makes use of future as well as current information to exact a particular system trajectory, future and current information are both available (and useful) to a state estimator which employs a closed-loop model of the preview-controlled system. Background on the Optimal Preview Controller